// -*- coding: utf-8 -*-

#include <iostream>
#include <fstream>
#include <cmath>
#include <boost/shared_array.hpp>

#include "elisa/elisa_interval.h"

#include "hybrid_solver.h"
#include "hybrid_newton_reduction1.h"
#include "hybrid_util.h"

using namespace vnodelp;
using namespace v_bias;
using namespace v_blas;

USING_ELISA

//--------------------------------------------------------------------------
// class HybridReduction
//--------------------------------------------------------------------------

const double HybridNewtonReduction1::MinH(1.0e-13);
const int    HybridNewtonReduction1::Ratio(10);
const double HybridNewtonReduction1::Factor(1.0-1.0e-1);

bool HybridNewtonReduction1::dump(false);

bool HybridNewtonReduction1::reductionAlgorithm()
{
#ifdef HYBRID_PROFILE
	++numHybridReduction;
#endif

	// 必要な情報を集める
	ContConstraintRep *rep = m_constr.rep();
	Time&      time = rep->getTime();
	TimeImpl  *ti   = dynamic_cast<TimeImpl *>(time.getImplementation().get());
	// 時間ドメイン
	VNInterval td(ti->domain().inf(), ti->domain().sup());

	// 連続状態ドメイン (を入れる変数)
	VNIntVec   aX;

#ifdef _DEBUG
	cout << "narrowing... ";
#endif

	// 初期状態の集合
	VNIntVec   x(time.getInitValue());
	// 初期時間の集合
	VNInterval minT(time.getInitTime());

	// 時間ドメインからexcessの分を削減する
	VNInterval excess(minT + time.getExcess());
	if (vn::isPossiblyLessThan(excess, td)) {
		if (!vn::isCertainlyLessThan(excess, td))
			td = VNInterval(sup(excess), sup(td));
	} else
		return false;

	// 時間ドメインの下端の状態を求める
	//double maxT = ti->domain().inf() + width(minT);
	double maxT = inf(td);
	double offset = maxT - sup(minT);

	if (offset > 0) {
		//if (offset >= MinH) {
		m_odeSolver.setFirstEntry();
		m_odeSolver.setHmax(width(minT) + offset);
		m_odeSolver.setOneStep(off);
		m_odeSolver.integrate(minT, x, maxT);
	}

	// 20090402 VNInterval t(inf(minT), sup(td));
	//VNInterval t(td);

#ifndef FIRST_SOL_ONLY
	// 右側を削減
	if (!narrow(false, x, td, width(minT), aX))
		return false;
	// 左側を削減
	if (!narrow(true,  x, td, width(minT), aX))
		return false;
#else
	// 左側を削減
	if (!narrow(true,  x, td, width(minT), aX))
		return false;
#endif

#ifdef _DEBUG
	// 削減結果の出力
	cout << "finished." << endl << "t: " << td << endl;
#endif

	ti->setDomain(td);

	// ODE求解結果をElisaドメインに伝播する
	const ContVarVec & cvv = rep->getContVars();
	for (int i(0); i < cvv.size(); ++i) {
		ContVarImpl *cv = dynamic_cast<ContVarImpl *>(cvv[i]->getImplementation().get());

		Interval d(aX[i]);
		d.intersect(cv->domain());
		if (d.isEmpty())
			return false;
		cv->setDomain(d);
	}

	return true;
}

// 左側あるいは右端を削減する
bool HybridNewtonReduction1::narrow(const bool left, VNIntVec& x, VNInterval& t, const double &wT, VNIntVec& aX)
{
#ifdef HYBRID_PROFILE
	++numNarrow;
#endif

	// ガード条件の判定
	VNInterval t1(inf(t) + vn::Unit*wT);
	VNIntVec x1(x);
	aX.assign(x1.begin(), x1.end());
	m_odeSolver.setFirstEntry();
	m_odeSolver.setHmax(width(t));
	m_odeSolver.setOneStep(on);
	double maxT(sup(t));
	while (sup(t1) < maxT) {
		m_odeSolver.integrate(t1, x1, maxT);
		// a priori包囲をためこむ
		VNIntVec aX1 = m_odeSolver.getAprioriEncl();
		transform(aX.begin(), aX.end(), aX1.begin(), aX.begin(), vn::hull);
	}

#ifdef _DEBUG
	cout << "encl: " << t << ' : ' << aX[0] << endl;
#endif

	if (!m_constr.getGFunc()(aX, t).set_contains(VNInterval::zero))
		return false;

	// tを削減する
	x1.assign(x.begin(), x.end());
	t1 = t;
	if (!newtonIt(x1, t1, wT, aX))
		return false;

#ifdef _DEBUG
	cout << "newt: " << t1 << endl;
#endif

	/*
	// t1が十分に削減された
	if (t1.width() <= MinH*Ratio) {
		if (left) {
			x.assign(x1.begin(), x1.end()); 
			t = vn::hull(t1, sup(t));
		} else {
			t = vn::hull(inf(t), t1);
		}

		return true;
	}
	*/

	// 左端 (右端) の状態を求める

	VNInterval t2(inf(t) + vn::Unit*wT);

	maxT = left ? inf(t1) + wT : sup(t1);
	if (sup(t2) != maxT) {
		x1.assign(x.begin(), x.end()); 
		m_odeSolver.setFirstEntry();
		m_odeSolver.setHmax(v_bias::width(t));
		m_odeSolver.setOneStep(off);
		m_odeSolver.integrate(t2, x1, maxT);
	}

	// 端から最短幅で解く

	m_odeSolver.setFirstEntry();
	m_odeSolver.setHmax(wT + MinH*Ratio); // TODO
	m_odeSolver.setOneStep(on);

	VNIntVec x2(x1);

	m_odeSolver.integrate(t2, x2, left ? sup(t) : inf(t));
	VNIntVec aX1(m_odeSolver.getAprioriEncl());
	t2 = m_odeSolver.getT();

	// 端でのガード条件の評価
	if (m_constr.getGFunc()(aX1, t2).set_contains(VNInterval::zero) 
		|| width(t1) <= MinH*Ratio) {

#ifndef FIRST_SOL_ONLY
		if (left) {
			x.assign(x1.begin(), x1.end()); 
			t = vn::hull(t1, sup(t));
		} else {
			t = vn::hull(inf(t), t1);
		}
#else
		t =	t2;
#endif

	} else {
		//++bc;

		// tを分割する
		VNInterval l, r;
		t1.split(l, r);

		if (left) {
			if (narrow(true, x1, l, 0.0, aX1)) { // L-L
				x = x1;
				t = vn::hull(l, sup(t));

			} else { // L-R
				// 右側の下端値を求める
				t1 = inf(t) + vn::Unit*wT; 
				x1.assign(x.begin(), x.end()); 
				m_odeSolver.setFirstEntry(); 
				m_odeSolver.setHmax(width(t)); 
				m_odeSolver.setOneStep(off); 
				m_odeSolver.integrate(t1, x1, inf(r) + wT); 

				if (narrow(true, x1, r, width(t1), aX1)) {
					x = x1;
					t = vn::hull(r, sup(t));
				} else
					return false;
			}
		} else {
			// 右側の下端値を求める
			t1 = inf(t) + vn::Unit*wT;
			x1.assign(x.begin(), x.end()); 
			m_odeSolver.setFirstEntry();
			m_odeSolver.setHmax(v_bias::width(t));
			m_odeSolver.setOneStep(off);
			m_odeSolver.integrate(t1, x1, inf(r) + wT);

			if (narrow(false, x1, r, width(t1), aX1)) { // R-R
				t = vn::hull(inf(t), r);
			
			} else { // R-L
				// 左側の下端値を求める
				x1.assign(x.begin(), x.end()); 
				if (inf(t) != inf(l)) {
					t1 = inf(t) + vn::Unit*wT;
					m_odeSolver.setFirstEntry();
					m_odeSolver.setHmax(width(t));
					m_odeSolver.setOneStep(off);
					m_odeSolver.integrate(t1, x1, inf(l) + wT);
				}

				if (narrow(false, x1, l, width(t1), aX1)) {
					t = vn::hull(inf(t), l);
				} else	
					// 不要？
					return false;
			}
		}
	}
	return true;
}

// 不動点に達するまでニュートン法を適用する
bool HybridNewtonReduction1::newtonIt(VNIntVec& x, VNInterval& t, const double &wT, VNIntVec& aX)
{
	VNInterval t1(inf(t) + vn::Unit*wT);
	VNIntVec   x1(x);
	VNIntVec   nextX(x);
	VNInterval nextT(t);

	double     wT1(wT);

	VNIntVec   midX;
	VNInterval midT;

	while (!nextT.is_empty() && width(nextT) - wT > MinH*(Ratio)) {
#ifdef HYBRID_PROFILE
		++numNewton;
#endif

		// 最終状態を求める
		aX.assign(nextX.begin(), nextX.end());
		x1.assign(nextX.begin(), nextX.end());
		wT1 = width(t1); // TODO
		//t = inf(nextT) + v_bias::Unit()*w;
		midX = x1;

		m_odeSolver.setFirstEntry();
		m_odeSolver.setHmax(wT1 + (width(nextT)-wT1)/2 + MinH); // 最大ステップ幅はnextTの幅の半分 (初期時間幅も考慮)
		m_odeSolver.setOneStep(on);

#ifdef HYBRID_PROFILE
		static const int li(0);

#endif
		// 初期値のロギング
		if (dump)
			dumpPipe(nrwOut, m_constr, t1, x1);

		// T上の連続軌道を計算する
		bool b(true);
		while (sup(t1) < sup(nextT)) {
			m_odeSolver.integrate(t1, x1, sup(nextT));

			// a priori包囲をためこむ
			VNIntVec aX1 = m_odeSolver.getAprioriEncl();
			transform(aX.begin(), aX.end(), aX1.begin(), aX.begin(), vn::hull);

			// 中心 (付近) での状態を記録する
			if (b && inf(t1) >= v_bias::midpoint(nextT)) {
				b = false;
				midX = x1;
				midT = t1;
			}


			if (dump) {
				// ロギング
				//dumpPipe(nrwOut, m_constr, t1, x1);

				v_bias::interval Tj = m_odeSolver.getT();
				dumpPipe(nrwOut, m_constr, Tj, aX1);

				//v_bias::printAsRect(outA, Tj, aX1[2]);
				//v_bias::printAsRect(outA, aX1[0], aX1[2]);
				//v_bias::printAsRect(outA, aX1[0], aX1[1]);
			}

		}
#ifdef HYBRID_PROFILE
		//odeOut << std::endl;
#endif

		// 削減前のTを保存
		x = nextX;
		t = nextT;

		// 区間ニュートン法によるTの削減
		VNInterval h(m_constr.getGFunc()(midX, midT));
		VNInterval dh(0.0);

		{
		const int n(aX.size());

		// 配列
		boost::shared_array<VNInterval> axa(new VNInterval[n]);
		// 自動微分用変数ベクトル
		AD_VNIntVec adv(n);
		
		for (int i(0); i < aX.size(); i++) {
			axa[i] = aX[i];
			adv[i] = AD_VNInterval(aX[i]);
		}

		// 時間
		AD_VNInterval adt(t);

		// 自動微分用DAGを作成
		AD_VNInterval gfunc(m_constr.getDGFunc()(adv, adt));
		gfunc.diff(0, 1);

		// 軌道の微分値を計算
		boost::shared_array<VNInterval> dtraj(new VNInterval[n]);
		m_constr.getDFunc()(n, dtraj.get(), axa.get(), aX[0], NULL);

		dh = adt.d(0);
		for (int i(0); i < adv.size(); i++) {
				dh += adv[i].d(0) * dtraj[i];
		}
		}

		if (dh.set_contains(0.0))
			return true;

		v_bias::intersect(nextT, nextT, (midT - h/dh));

#ifdef _DEBUG
		// 削減結果の出力
		cout << "t0: " << t << endl;
		cout << "t:  " << nextT << endl;
		cout << "existence: " << t.set_strictly_contains(nextT) << endl;
#endif

		// 20090325
		double w(width(nextT));
		if (width(nextT) < MinH) {
			double s(inf(nextT)+MinH);
			nextT = VNInterval(inf(nextT), inf(nextT)+MinH);
			//return true;
		}

		// 初期状態を更新
		t1 = inf(t) + vn::Unit*wT1;
		// 20090321 if (inf(nextT) != inf(t)) { // 下限側が削減された
		if (std::abs(inf(nextT) - inf(t)) >= MinH) { // 下限側が削減された
			// 削減後の状態を計算
			nextX.assign(x.begin(), x.end());
			m_odeSolver.setFirstEntry();
			m_odeSolver.setOneStep(off);
			m_odeSolver.setHmax(width(t));
			m_odeSolver.integrate(t1, nextX, inf(nextT) + wT1);
		
		} else { // 削減幅が少ない場合は削減しない
			if (!nextT.is_empty())
				nextT = VNInterval(inf(t), sup(nextT));
		}
 
		if (width(nextT) >= width(t)*Factor)
			break;
	}

	if (nextT.is_empty())
		return false;

	x = nextX;
	t = nextT;
	return true;
}
